#pragma once

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>

class RepublishMapOdom : public rclcpp::Node
{
public:
    RepublishMapOdom() : Node("republish_map_odom")
    {
        amcl_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "/amcl_pose", 10,
            std::bind(&RepublishMapOdom::amclCallBack, this, std::placeholders::_1));
        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10,
            std::bind(&RepublishMapOdom::odomCallBack, this, std::placeholders::_1));
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
        pub_tf_ = this->create_publisher<tf2_msgs::msg::TFMessage>(
            "/tf", 10);
        // 创建定时器定期查询变换
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(10),
            std::bind(&RepublishMapOdom::rePublish, this));
    }

    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr amcl_pose_sub_;
    rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;

    void amclCallBack(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
    void odomCallBack(const nav_msgs::msg::Odometry::SharedPtr msg);
    void rePublish();
    rclcpp::TimerBase::SharedPtr timer_;

private:
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    geometry_msgs::msg::PoseWithCovarianceStamped amcl_pose_;
    nav_msgs::msg::Odometry odom_;
    rclcpp::Time amcl_time_;
};